﻿import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.SensorPortListener;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.LightSensor;
import lejos.nxt.TouchSensor;
import lejos.robotics.RegulatedMotor;
import lejos.util.Delay;


public class main01
{
    private final SensorPort ultrasonicPort;
    private final UltrasonicSensor ultrasonicSensor;
    private boolean sonicIsOn = true;
    private boolean done;
    private final SensorPort lightPort;
    private final LightSensor lightSensor;
    private boolean lightIsOn = true;
    
    private int direction = 1;
    
    public static void main(String[] args)
    {
            final main01 robot;
            robot = new main01();
            robot.run();
    }
    
    public main01()
    {
            ultrasonicPort = SensorPort.S1;
            ultrasonicSensor = new UltrasonicSensor(ultrasonicPort);
            
            lightPort = SensorPort.S4;
            lightSensor = new LightSensor(lightPort);
            lightSensor.setFloodlight(true);
    }
    
    public void run()
    {
            // Setting Defaults
            int distance;
            Motor.B.setSpeed(360);
            Motor.A.setSpeed(365);
            
            
            
            
            
            // Down the First Hall
            moveForwards();
            // <RIGHT> First Turn
            waitForDistance(12);
            turnRight90();
            
            moveForwards();
            // complete First turn
            waitForDistance(12);
            turnRight90();
            
            moveForwards();
            
            waitForDistance(12);
            turnRight90();
            
            moveForwards();
            
            
            
            
            // Down the Second Hall
            moveForwards();
            // <LEFT> Second Turn
            waitForDistance(12);
            turnLeft90();
            
            moveForwards();
            // complete Second turn
            waitForDistance(12);
            turnLeft90();
            
            moveForwards();


    }
    //*****************//
    private void moveForwards()
    {
            if(done == true)
            {
                    done = false;
            }
            Motor.B.forward();
            Motor.A.forward();
            
            
    }
    
    private void reverse(int x)
    {
            if(done == true)
            {
                    done = false;
            }
            x = x * -1;
                    Motor.B.rotate(x, true);
                    Motor.A.rotate(x);    
    }
    
    //*****************//
    
    
    //turning
    //*****************//
    public void turnLeft90()
    {
            if(done == true)
            {
                    done = false;    
            }
                    Motor.B.rotate(158 * 1, true);
                    Motor.A.rotate(-158 * 1);
    }
    
    public void turnRight90()
    {
            if(done == true)
            {
                    done = false;
            }
                    Motor.B.rotate(-158 * direction, true);
                    Motor.A.rotate(158 * 1);
    }


    public void right180()
    {
            if(done == true)
            {
                    done = false;
            }
                    Motor.B.rotate(-158 * 2, true);
                    Motor.A.rotate(158 * 2);
    }
    public void left180()
    {
            if(done == true)
                    {
                            done = false;
                    }
                            Motor.B.rotate(-158 * 2, true);
                            Motor.A.rotate(158 * 2);
    }
    public void spin()
    {
            if(done == true)
            {
                    done = false;
            }
                    Motor.B.rotate(-158 * 8, true);
                    Motor.A.rotate(158 * 8);
    }
    
    //**************//
    
    //************************//
    // stopp stuff
    private void stop()
    {
            synchronized(this)
            {
                    System.out.println("Trigger Touch Listener");
                    done = true;
                    
                    reverse(77);
            }
    }
    
    private int waitForDistance(final int max)
    {
            int distance;
            
            do
            {
                    try
                    {
                            // pause 100 ms between reads...
                            // this is a LeJOS issue
                            // later versions of LeJOS won't require this
                            Thread.sleep(100);
                    }
                    catch (InterruptedException ex)
                    {
                    }
                    
                    distance = ultrasonicSensor.getDistance();
            }
            
            while(distance > max);
            return (distance);
    }
    
    private int waitForLight(final int max)
    {
            int light;
            
            do
            {
                    light = lightSensor.readNormalizedValue();
            } while(light < max);
            
            return (light);
    }
}